using System;
using System.Collections.Generic;
using System.Linq;
using Priority_Queue;
using UnityEngine;

namespace Gj.Pathfinding
{
    public class Dijkstra
    {
        public Func<VisitedNode, VisitedNode, double> AStarCost = (node, end) =>
        {
            return node.MinCostToStart + node.Node.Distance(end.Node);
        };

        public Func<VisitedNode, VisitedNode, double> BaseCost = (node, end) => { return node.MinCostToStart; };
        public Queue<VisitedNode> VisitedNodes;

        public Dijkstra()
        {
            VisitedNodes = new Queue<VisitedNode>();
        }

        public float ToDijkstra(Node startNode, Node endNode, int mask, ref List<Node> shortestPath,
            Func<VisitedNode, VisitedNode, double> costFunc)
        {
            var start = VisitedNodes.Count > 0 ? VisitedNodes.Dequeue() : new VisitedNode();
            start.Node = startNode;
            start.MinCostToStart = -1;
            start.NearestToStart = null;

            var end = VisitedNodes.Count > 0 ? VisitedNodes.Dequeue() : new VisitedNode();
            end.Node = endNode;
            end.MinCostToStart = -1;
            start.NearestToStart = null;
            float cost = 0;
            var r = DijkstraSearch(start, end, mask, costFunc);
            // Debug.LogErrorFormat("path:"+r);
            if (r)
            {
                shortestPath.Add(endNode);
                cost = BuildShortestPath(shortestPath, end);
                shortestPath.Reverse();
            }

            // Console.Write(shortestPath.Count);
            return cost;
        }

        private float BuildShortestPath(List<Node> list, VisitedNode end)
        {
            var node = end;
            float cost = 0;
            while (node.NearestToStart != null)
            {
                list.Add(node.NearestToStart.Node);
                // Console.WriteLine(node.NearestToStart.Node.Point + ":"+node.Node.Point);
                cost += node.Cost;
                var tmp = node.NearestToStart;
                node.Visited = false;
                VisitedNodes.Enqueue(node);
                node = tmp;
            }

            return cost;
        }

        private bool DijkstraSearch(VisitedNode start, VisitedNode end, int mask,
            Func<VisitedNode, VisitedNode, double> cost)
        {
            var prioQueue = new NodePriorityQueue<VisitedNode>();
            prioQueue.Enqueue(start, 0);
            var visited = new Dictionary<Vector2Int, VisitedNode>();
            visited.Add(start.Node.Point(), start);
            visited.Add(end.Node.Point(), end);
            // var endLink = new Dictionary<Node, Edge>();
            // foreach (var cnn in end.Node.Connections)
            // {
            //     endLink.Add(cnn.ConnectedNode, cnn);
            //     cnn.ConnectedNode = end.Node;
            // }

            do
            {
                var node = prioQueue.Dequeue();

                if (node.Node.Point() == end.Node.Point())
                    return true;
                var connections = node.Node.Connections();
                Edge edge;
                // if (endLink.TryGetValue(node.Node, out edge))
                // {
                //     connections = connections.GetRange(0, connections.Count);
                //     connections.Add(edge);
                // }

                foreach (var cnn in connections.OrderBy(x => x.Cost))
                {
                    var childNode = cnn.ConnectedNode;
                    VisitedNode visitedNode;
                    if (visited.TryGetValue(childNode.Point(), out visitedNode))
                    {
                        if (visitedNode.Visited) continue;
                    }
                    else
                    {
                        visitedNode = VisitedNodes.Count > 0 ? VisitedNodes.Dequeue() : new VisitedNode();
                        visitedNode.Visited = false;
                        visitedNode.NearestToStart = null;
                        visitedNode.MinCostToStart = -1;
                        visitedNode.Node = childNode;
                        visited.Add(childNode.Point(), visitedNode);
                    }

                    if (childNode.Status() != mask)
                        continue;
                    if (visitedNode.MinCostToStart < 0 ||
                        node.MinCostToStart + cnn.Cost < visitedNode.MinCostToStart)
                    {
                        visitedNode.MinCostToStart = node.MinCostToStart + cnn.Cost;
                        visitedNode.NearestToStart = node;
                        visitedNode.Cost = cnn.Cost;
                        // Console.WriteLine("edge"+cnn.ConnectedNode.Point);
                        if (!prioQueue.Contains(visitedNode))
                            prioQueue.Enqueue(visitedNode, cost(visitedNode, end));
                    }
                }

                node.Visited = true;
            } while (prioQueue.Any());

            return false;
        }
    }

    public class Edge
    {
        public float Cost { get; set; }
        public Node ConnectedNode { get; set; }
    }

    public interface Node
    {
        List<Edge> Connections();
        Vector2Int Point();
        int Status();
        double Distance(Node node);
    }

    public class LinkNode: Node
    {
        public int _Status { get; set; }
        public Vector2Int _Point { get; set; }
        public List<Edge> _Connections { get; set; } = new();

        public List<Edge> Connections()
        {
            return _Connections;
        }

        public Vector2Int Point()
        {
            return _Point;
        }

        public int Status()
        {
            return _Status;
        }

        public double Distance(Node end)
        {
            var en = end.Point();
            return Math.Sqrt(Math.Pow(this._Point.x - en.x, 2) + Math.Pow(this._Point.y - en.y, 2));
        }
        
        internal void ConnectedNode(LinkNode node, int cost)
        {
            _Connections.Add(new Edge
                {
                    ConnectedNode = node,
                    Cost = cost
                });
        }

        internal void ConnectDuplex(List<LinkNode> nodes)
        {
            var connections = new List<Edge>();
            foreach (var node in nodes)
            {
                if (node._Point == _Point)
                    continue;

                // var dist = Math.Sqrt(Math.Pow(Point.x - node.Point.x, 2) + Math.Pow(Point.y - node.Point.y, 2));
                var dist = Math.Abs(_Point.x - node._Point.x) + Math.Abs(_Point.y - node._Point.y);
                connections.Add(new Edge
                {
                    ConnectedNode = node,
                    Cost = dist
                });
            }

            connections = connections.OrderBy(x => x.Cost).ToList();
            foreach (var cnn in connections)
            {
                //Connect three closes nodes that are not connected.
                if (_Connections.All(c => c.ConnectedNode != cnn.ConnectedNode))
                    _Connections.Add(cnn);

                //Make it a two way connection if not already connected
                if (((LinkNode)cnn.ConnectedNode)._Connections.All(cc => cc.ConnectedNode != this))
                {
                    var backConnection = new Edge { ConnectedNode = this, Cost = cnn.Cost };
                    ((LinkNode)cnn.ConnectedNode)._Connections.Add(backConnection);
                }
            }
        }

        internal void ConnectSimplex(List<LinkNode> nodes)
        {
            foreach (var node in nodes)
            {
                if (node._Point == _Point)
                    continue;

                // var dist = Math.Sqrt(Math.Pow(Point.x - node.Point.x, 2) + Math.Pow(Point.y - node.Point.y, 2));
                var dist = Math.Abs(_Point.x - node._Point.x) + Math.Abs(_Point.y - node._Point.y);
                _Connections.Add(new Edge
                {
                    ConnectedNode = node,
                    Cost = dist
                });
            }

            _Connections = _Connections.OrderBy(x => x.Cost).ToList();
        }

        internal void ConnectDuplex(LinkNode node)
        {
            var dist = Math.Abs(_Point.x - node._Point.x) + Math.Abs(_Point.y - node._Point.y);
            _Connections.Add(new Edge
            {
                ConnectedNode = node,
                Cost = dist
            });
            if (node._Connections.All(cc => cc.ConnectedNode != this))
            {
                var backConnection = new Edge { ConnectedNode = this, Cost = dist };
                node._Connections.Add(backConnection);
            }
        }

        internal void DisconnectSimplex(LinkNode node)
        {
            _Connections.RemoveAll(ee => ee.ConnectedNode == node);
        }

        internal void DisconnectDuplex(List<LinkNode> nodes)
        {
            foreach (var node in nodes)
            {
                node.DisconnectSimplex(this);
                DisconnectSimplex(node);
            }
        }

    }

    public class VisitedNode
    {
        public Node Node;

        public double MinCostToStart { get; set; } = -1;
        public VisitedNode NearestToStart { get; set; }
        public bool Visited { get; set; }

        public float Cost { get; set; }
    }

    public interface Map
    {
        bool MoveNext();
        Node Current();
        void Dispose();
    }

    public class NodePriorityQueue<TItem> : SimplePriorityQueue<TItem, double>
    {
        /// <summary>
        ///     Instantiate a new Priority Queue
        /// </summary>
        public NodePriorityQueue()
        {
        }

        /// <summary>
        ///     Instantiate a new Priority Queue
        /// </summary>
        /// <param name="comparer">The comparer used to compare priority values.  Defaults to Comparer&lt;float&gt;.default</param>
        public NodePriorityQueue(IComparer<double> comparer) : base(comparer)
        {
        }

        /// <summary>
        ///     Instantiate a new Priority Queue
        /// </summary>
        /// <param name="comparer">The comparison function to use to compare priority values</param>
        public NodePriorityQueue(Comparison<double> comparer) : base(comparer)
        {
        }
    }
}